# -- coding:utf-8 --
#!/usr/bin/python
import sys, os
sys.path.append("/home/avatar/Minibot/04.RobotSamples/02.CarRobot")
import inspect
from BwSensorLib import BwSensorLib
from CarRobot import CarRobot
from BwRobotLib import BwRobotLib
from MiniBotControlLib import MiniBotControlLib
import time
import BwHelpLib 

msg = """
请选择轮子模式 :
2 : 两轮
3 : 三轮
4 : 四向轮
5 : 四麦克朗姆轮
6 : 四并橡胶轮
CTRL-C to quit
"""

cmd = """
    命令：
    1 : 旋转指定角度
    2 : 设置移动速度 米/秒
    3 : 移动默认10秒
    4 : 设置旋转速度 弧度/秒
    5 : 旋转默认3秒
    Ctrl+C : 退出
"""

class CarCommandCtl:
    def __init__(self):
        # 实例化MiniBotControlLib类，并设置为udp模式
        self.mlib = MiniBotControlLib("udp")
        # 输入ip地址和端口号
        self.ip = "10.10.100.254"
        self.port = 8899
        # 连接机器人,并设置超时时间2s
        self.mlib.Connect(self.ip, self.port, 2000)
        # 实例化BwRobotLib类，并将mlib作为参数传入
        self.robotlib = BwRobotLib(self.mlib)
        # 实例化carRobot类，并将mlib作为参数传入
        self.carRobot = CarRobot(self.mlib)
        # 实例化传感器库对象
        self.sensorlib = BwSensorLib(self.mlib)
        # 根据模块连接位置，自动加载ID配置,print_flag设为True，可打印打印当前模块信息
        self.robotlib.module_ids = self.robotlib.scanNConfigCarIds(print_flag=True)
        # 配置轮式模式 2:两轮   3:三轮   4:四向轮  5:四麦克朗姆轮  6:四并橡胶轮
        print(msg)
        self.wheel_mode = BwHelpLib.inputNum()
        
        # 配置机器人（包括ID位置与速度模式）
        # 配置ID:ids F(4),B(11),l1(8),l2(8),l3(10),l4(11),r1(4),r2(7),r3(6),r4(6) 【TOP:9】
        self.carRobot.bindIds(self.robotlib.module_ids, self.wheel_mode)
        # 设置小车速度
        self.speed = 50
        self.carRobot.update_speed(self.speed)


    #处理小车旋转指令
    def handleRotateCmd(self):
        print("请输入要旋转的角度:")
        angle = BwHelpLib.inputNum()
        self.carRobot.rotate(angle)

    #处理小车速度指令，单位：米/秒
    def handleSetSpeedCmd(self):
        print("请输入速度米/秒:")
        speed = BwHelpLib.inputNum(float)
        self.carRobot.setRunningSpeedWithMS(speed)

    #处理小车速度指令，单位：弧度/秒
    def handleSetRotateSpeedCmd(self):
        print("请输入速度弧度/秒:")
        angleSpeed = BwHelpLib.inputNum(float)
        radSpeed = angleSpeed * 3.14 / 180
        self.carRobot.setRunningSpeedWithRadS(radSpeed)

    def run(self):
        cmd = BwHelpLib.inputNum()
        if(cmd == 0):
            break
        if(cmd == 1):
            self.handleRotateCmd()
        if(cmd == 2):
            self.handleSetSpeedCmd()
        if(cmd == 3):
            print("输入秒数 (默认10秒):")
            seconds = BwHelpLib.inputNum()
            if(seconds <= 0):
                seconds = 10
            self.carRobot.goForward()
            time.sleep(5)
            self.carRobot.stop()
        if(cmd == 4):
            self.handleSetRotateSpeedCmd()
        if(cmd == 5):
            print("输入秒数 (默认3秒):")
            seconds = BwHelpLib.inputNum()
            if(seconds <= 0):
                seconds = 3
            self.carRobot.turnLeft()
            time.sleep(seconds)
            self.carRobot.stop()

if __name__ == "__main__":
    try :
        car = CarCommandCtl()    
        print(cmd)
        while(1):
            key = BwHelpLib.readkey()
            car.run()     
            if (key == '\x03'): # Ctrl+C 退出程序
                break        
    except KeyboardInterrupt:
        print(" Program closed! ")
    pass

